#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

#commom lib
import math
import numpy as np
import time
import datetime
import cv2
import os
from time import sleep
from yahboomcar_laser.common import *

print ("improt done")
RAD2DEG = 180 / math.pi


class laserTracker(Node):
    def __init__(self,name):
        super().__init__(name)
        #create a sub
        self.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)
        self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)
        #create a pub
        self.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)
        
        #declareparam
        self.declare_parameter("linear",0.5)
        self.linear = self.get_parameter('linear').get_parameter_value().double_value
        self.declare_parameter("angular",1.0)
        self.angular = self.get_parameter('angular').get_parameter_value().double_value
        self.declare_parameter("LaserAngle",20.0)
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.declare_parameter("ResponseDist",0.5)
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
        self.declare_parameter("Switch",False)
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        
        # 容差距离
        self.distance_tolerance = 0.15
     
        self.Joy_active = False
        self.ros_ctrl = SinglePID()
        self.priorityAngle = 60  # 40
        self.Moving = False
        
        # 调整PID参数以适应新的控制策略
        # 距离控制PID - 只控制角速度来调节距离
        self.distance_pid = SinglePID(1.5, 0.0, 0.8)
        # 角度控制PID - 保持朝向目标
        self.angle_pid = SinglePID(1.5, 0.0, 0.9)
        
        # 恒定的切向速度
        self.tangential_speed = 0.25
        # 默认角速度
        self.default_av = 1.5

        self.frame_count=0
        self.cap = cv2.VideoCapture(0)
        if not self.cap.isOpened():
            print("无法打开摄像头")
            raise Exception("无法打开摄像头")

        self.distance_clamp_min=0.2
        self.distance_clamp_max=3.0

        # 摄像机模式
        # 创建带时间戳的子目录
        timestamp_dir = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
        self.save_dir = os.path.join(
            "/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/image",
            timestamp_dir
        )
        os.makedirs(self.save_dir, exist_ok=True)


        # 尝试设置为摄像头支持的最大分辨率（如 1920x1080）
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)

        width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        print(f"当前摄像头分辨率: {width}x{height}")


        
        
        self.timer = self.create_timer(0.01,self.on_timer)
        self.timer_capture = self.create_timer(0.5,self.capture_camera)
    
    def capture_camera(self):
        print("保存照片")
        ret, frame = self.cap.read()
        if not ret:
            print("读取帧失败")
            return
        self.frame_count+=1
        timestamp = time.strftime("%Y%m%d_%H%M%S", time.localtime())
        filename = os.path.join(self.save_dir, f"{timestamp}_{self.frame_count:04d}.jpg")
        success = cv2.imwrite(filename, frame)
        if success:
            print(f"成功保存图像: {filename}")
        else:
            print(f"图像保存失败: {filename}")

    def on_timer(self):
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        self.angular = self.get_parameter('angular').get_parameter_value().double_value
        self.linear = self.get_parameter('linear').get_parameter_value().double_value
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value

    def JoyStateCallback(self, msg):
        if not isinstance(msg, Bool): return
        self.Joy_active = msg.data

    def get_direction(self,distance_list, angle_list):
        # 确保数据列表不为空
        if len(distance_list) > 0:
            # 排序距离，选取前1/2的最小距离
            sorted_indices = np.argsort(distance_list)  # 获取按距离升序排列的索引
            half_length = len(sorted_indices) // 2  # 获取前1/2的点数
            selected_indices = sorted_indices[:half_length]  # 选择前1/2的点

            # 获取选取的距离和角度
            selected_distances = np.array([distance_list[i] for i in selected_indices])
            selected_angles = np.array([angle_list[i] for i in selected_indices])

            # 确保有选取的点
            if len(selected_distances) > 0:
                # 对角度进行加权计算（近距离的角度权重大）
                weights = 1.0 / selected_distances  # 距离的倒数作为权重
                weighted_angle = np.average(selected_angles, weights=weights)  # 加权平均计算方位角度
                
                # 返回最小距离和加权后的方位角度
                min_distance = selected_distances.min()  # 返回最短距离
                return min_distance, weighted_angle
            
        return 999, 999

    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        offset = 0.5
        
        velocity = Twist()
        
        frontDistList = []
        frontDistIDList = []
        minDistList = []
        minDistIDList = []

        
        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.priorityAngle):
            	if ranges[i] < (self.ResponseDist + offset):
            		frontDistList.append(ranges[i])
            		frontDistIDList.append(angle)
            elif (180 - self.LaserAngle) < angle < (180 - self.priorityAngle):
            	minDistList.append(ranges[i])
            	minDistIDList.append(angle)
            elif (self.priorityAngle - 180) < angle < (self.LaserAngle - 180):
            	minDistList.append(ranges[i])
            	minDistIDList.append(angle)


        if len(frontDistIDList) != 0:
        	minDist = min(frontDistList)
        	minDistID = frontDistIDList[frontDistList.index(minDist)]
        elif minDistList:
        	minDist = min(minDistList)
        	minDistID = minDistIDList[minDistList.index(minDist)]
        else:
            minDist = self.ResponseDist
            minDistID=180        



            

        print(f"方位角{minDistID}")

        if self.Joy_active or self.Switch == True:
        	if self.Moving == True:
        		self.pub_vel.publish(Twist())
        		self.Moving = not self.Moving
        	return

        self.Moving = True
        
        
        # 计算转弯半径 R
        base_angular_velocity = 0.0
        base_angular_velocity = -self.tangential_speed/(self.ResponseDist) # 将角度转换为弧度
        

        # 朝向修正项
        angle_error = (180 - abs(minDistID)) / 72  # 标准化角度误差
        angle_correction = self.angle_pid.pid_compute(angle_error, 0)
    
        if abs(angle_correction) < 0.02: 
            angle_correction = 0.0
        
        # 2. 距离调节角速度：通过改变转向速度来调节距离
        # 距离误差处理
        if abs(minDist - self.ResponseDist) < 0.06: 
            minDist = self.ResponseDist
        distance_error = minDist - self.ResponseDist
        distance_correction = 0.0
        if abs(distance_error) > self.distance_tolerance:
            # 距离过近时减慢角速度，距离过远时加快角速度
            distance_correction = self.distance_pid.pid_compute(self.ResponseDist, minDist)*0.12
            # distance_correction = max(min(distance_correction, 0.5), -0.5)
        
        correction_value=distance_correction + angle_correction
        # 3. 合成最终角速度
        final_angular_velocity = base_angular_velocity + correction_value
        
        # print(f"mindist:{minDist},base:{base_angular_velocity},distance_correction{distance_correction},angle_correction{angle_correction}")
        
        # 限制角速度范围
        final_angular_velocity = max(min(final_angular_velocity, 2.0), -2.0)
        # 设置运动指令
        velocity.linear.x = 0.0  # 移除 x 方向移动
        velocity.linear.y = self.tangential_speed  # 恒定切线速度
        velocity.angular.z = final_angular_velocity
        
        # print(f"运动指令参数 linear.y:{velocity.linear.y:.3f}, angular.z:{velocity.angular.z:.3f}")
        # print(f"距离误差:{distance_error:.3f}, 角度ID:{minDistID:.3f}, 当前角速度:{final_angular_velocity:.3f}, 距离校正:{distance_correction:.3f}")
        # if abs(distance_error) < 0.05 and abs(angle_correction) < 0.05:
        
        # self.capture_camera()
        self.pub_vel.publish(velocity)

    def shutdown_cleanup(self):
       """退出时的清理工作"""
       print("正在安全退出...")
       
       # 发送停止指令
       for _ in range(3):
           self.pub_vel.publish(Twist())
        #    time.sleep(0.05)
       print("已发送停止指令")

def main():
    rclpy.init()
    laser_tracker = laserTracker("laser_Tracker_a1")
    print ("start it")
    try:
        rclpy.spin(laser_tracker)
    except KeyboardInterrupt:
        pass
    finally:
        print("重置小车状态")
        laser_tracker.pub_vel.publish(Twist())
        laser_tracker.destroy_node()
        rclpy.shutdown()

import time

if __name__ == '__main__':
    main()